#include "Communication/NavigateToPoseActionClient.h"


namespace behavior_controller
{
std::shared_ptr<NavigateToPoseActionClient> navigate_to_pose_action_client = nullptr;

NavigateToPoseActionClient::NavigateToPoseActionClient(
        const rclcpp::Node::SharedPtr &ros_node, std::mutex *lock_, const std::string action_name)
        : ros_node_(ros_node)
{
    lock = lock_;
    nav_to_pose_client_ = rclcpp_action::create_client<ClientT>(ros_node_, action_name);
    send_goal_options = rclcpp_action::Client<ClientT>::SendGoalOptions();
    send_goal_options.result_callback =
            std::bind(&NavigateToPoseActionClient::resultCallback, this, std::placeholders::_1);
    send_goal_options.goal_response_callback =
            std::bind(&NavigateToPoseActionClient::goalResponseCallback, this, std::placeholders::_1);
    send_goal_options.feedback_callback =
            std::bind(&NavigateToPoseActionClient::feedback_callback, this, std::placeholders::_1, std::placeholders::_2);
};

void NavigateToPoseActionClient::sendGoal(geometry_msgs::msg::PoseStamped &pose_goal)
{
    client_goal.pose = pose_goal;
    std::thread{std::bind(&NavigateToPoseActionClient::work, this)}.detach();
}

void NavigateToPoseActionClient::work()
{
    rclcpp::Rate r(20);
    nav_to_pose_client_->async_send_goal(client_goal, send_goal_options);
    current_goal_status_ = ActionStatus::UNKNOWN;
    send_flag = true;
    time_send = ros_node_->now().seconds();
    while (rclcpp::ok())
    {
        if (is_cancel_requested)
        {
            auto cancel_future = nav_to_pose_client_->async_cancel_all_goals();
            RCLCPP_INFO(ros_node_->get_logger(), "NaviPlanner: cancel goal!");
            is_cancel_requested = false;
            break;
        }
        if (current_goal_status_ == ActionStatus::FAILED)
        {
            RCLCPP_WARN(ros_node_->get_logger(), "Failed to process waypoint ");
            break;
        }
        else if (current_goal_status_ == ActionStatus::SUCCEEDED)
        {
            RCLCPP_INFO(ros_node_->get_logger(), "Succeeded processing waypoint ");
            break;
        }
        if (runState() == ActionStatus::UNKNOWN && getFlag())
        {
            if (ros_node_->now().seconds() - time_send > 5)
            {
                current_goal_status_ = ActionStatus::FAILED;
                std::cout << "navigate_to_pose action client failed to send goal to server." << std::endl;
                break;
            }
        }
        r.sleep();
    }
}

void
NavigateToPoseActionClient::resultCallback(const rclcpp_action::ClientGoalHandle<ClientT>::WrappedResult &result)
{
    switch (result.code)
    {
        case rclcpp_action::ResultCode::SUCCEEDED:
            current_goal_status_ = ActionStatus::SUCCEEDED;
            return;
        case rclcpp_action::ResultCode::ABORTED:
            current_goal_status_ = ActionStatus::FAILED;
            return;
        case rclcpp_action::ResultCode::CANCELED:
            current_goal_status_ = ActionStatus::FAILED;
            return;
        default:
            current_goal_status_ = ActionStatus::UNKNOWN;
            return;
    }
}

void NavigateToPoseActionClient::feedback_callback(rclcpp_action::ClientGoalHandle<ClientT>::SharedPtr,
                                                   const std::shared_ptr<const ClientT::Feedback> feedback)
{
    std::cout << "feedback: distance_remaining= " << feedback->distance_remaining
              << " navigation_time.sec= " << feedback->navigation_time.sec << " number_of_recoveries= "
              << feedback->number_of_recoveries << std::endl;

    //feedback->current_pose;
}

void NavigateToPoseActionClient::goalResponseCallback(
        const rclcpp_action::ClientGoalHandle<ClientT>::SharedPtr &future)
{
    auto goal_handle = future.get();
    if (!goal_handle)
    {
        current_goal_status_ = ActionStatus::FAILED;
        std::cout << "navigate_to_pose action client failed to send goal to server." << std::endl;
        return;
    }
    current_goal_status_ = ActionStatus::PROCESSING;
    std::cout << "navigate_to_pose action client success to send goal to server." << std::endl;
}
}
